﻿using System;
using System.Collections.Generic;
using System.Windows;
using depthcamsensor = Microsoft.Robotics.Services.DepthCamSensor;
using webcamsensor = Microsoft.Robotics.Services.WebCamSensor;
using System.IO;
using Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Media.Media3D;
using System.Windows.Input;
using Microsoft.Robotics.Common;
using Microsoft.Robotics.PhysicalModel;

namespace KinectCamViewer
{
    /// <summary>
    /// Interaction logic for KinectCamViewerUI.xaml
    /// </summary>
    public partial class KinectCamViewerUI : Window
    {
        #region UI variables
        private KinectCamViewerOperations _mainPort;
        private int _currentFrame = 0;
        private int _maxCount = 0;
        private GeometryModel3D mGeometry;
        private MeshGeometry3D mesh;

        private bool mDown;
        private System.Windows.Point mLastPos;
        Material frontMaterial = new DiffuseMaterial(Brushes.SeaGreen);

        Transform3DGroup oldTransform = null;
        ModelVisual3D _oldVisualModel, visualModel;
        #endregion

        public KinectCamViewerUI(KinectCamViewerOperations mainPort)
        {
            _mainPort = mainPort;

            InitializeComponent();
        }

        private List<webcamsensor.WebCamSensorState> _webCamFrameList = new List<webcamsensor.WebCamSensorState>();
        private List<depthcamsensor.DepthCamSensorState> _depthCamFrameList = new List<depthcamsensor.DepthCamSensorState>();

        private byte[] ToByteArray(int width, int height, short[] depthData)
        {
            // NOTE: This code implicitly assumes that the width is a multiple
            // of four bytes because Bitmaps have to be longword aligned.
            byte[] buff = new byte[width * height * 3];
            byte val;
            int j = 0;
            for (int i = 0; i < width * height; i++)
            {
                // Convert the data to a suitable range
                if (depthData[i] >= (short)KinectReservedSampleValues.MaxDepthDataValue)
                {
                    val = byte.MaxValue;
                }
                else if (depthData[i] <= (short)KinectReservedSampleValues.MinValidDepth)
                {
                    val = byte.MaxValue;
                }
                else
                {
                    val = (byte)(((double)depthData[i] / (double)KinectReservedSampleValues.MaxDepthDataValue) * byte.MaxValue);
                }
                // Set all R, G and B values the same, i.e. gray scale
                buff[j++] = val;
                buff[j++] = val;
                buff[j++] = val;
            }

            return buff;
        }

        private void ShowDepthCamFrame(int index)
        {
            if (_depthCamFrameList.Count == 0 || index < 0 || index >= _depthCamFrameList.Count)
                return;

            var frame = _depthCamFrameList[index];

            byte[] data = ToByteArray(frame.DepthImageSize.Width, frame.DepthImageSize.Height, frame.DepthImage);

            _depthCamImage.Source = BitmapSource.Create(
                frame.DepthImageSize.Width,
                frame.DepthImageSize.Height,
                96, 96,
                PixelFormats.Bgr24,
                null,
                data,
                frame.DepthImageSize.Width * 3);
        }

        private void ShowWebCamFrame(int index)
        {
            if (_webCamFrameList.Count == 0 || index < 0 || index >= _webCamFrameList.Count)
                return;

            //_webCamImage.Source = _webCamFrameList[index].BitmapSource;
            var frame = _webCamFrameList[index];
            _webCamImage.Source = BitmapSource.Create(
                frame.Width,
                frame.Height,
                96, 96,
                PixelFormats.Bgr24,
                null,
                frame.Data,
                frame.Width * 3);
        }

        #region Event Handlers

        private void _openFile_Click(object sender, RoutedEventArgs e)
        {
            Microsoft.Win32.OpenFileDialog openDialog = new Microsoft.Win32.OpenFileDialog();
            openDialog.DefaultExt = "rec";
            openDialog.Filter = "rec files (*.rec)|*.rec";

            Nullable<bool> result = openDialog.ShowDialog();

            // Process open file dialog box results 
            if (result == true)
            {
                _depthCamFrameList.Clear();
                _webCamFrameList.Clear();
                _selectFrame.Items.Clear();
                _selectFrame.Text = "0";

                FileStream fs = new FileStream(openDialog.FileName, FileMode.Open);
                BinaryReader reader = new BinaryReader(fs);

                byte[] buffer = new byte[4];
                reader.Read(buffer, 0, 4);

                int depthFramesCount = BitConverter.ToInt32(buffer, 0);

                for (int i = 0; i < depthFramesCount; i++)
                {
                    depthcamsensor.DepthCamSensorState frame = new depthcamsensor.DepthCamSensorState();
                    frame.Deserialize(reader);

                    _depthCamFrameList.Add(frame);
                }

                reader.Read(buffer, 0, 4);
                int rgbFramesCount = BitConverter.ToInt32(buffer, 0);

                for (int i = 0; i < rgbFramesCount; i++)
                {
                    webcamsensor.WebCamSensorState frame = new webcamsensor.WebCamSensorState();
                    frame.Deserialize(reader);

                    _webCamFrameList.Add(frame);
                }

                _maxCount = Math.Max(_webCamFrameList.Count, _depthCamFrameList.Count);
                reader.Close();
                fs.Close();

                ShowDepthCamFrame(0);
                ShowWebCamFrame(0);

                for (int i = 0; i < _maxCount; i++)
                {
                    _selectFrame.Items.Add(i);
                }
                _selectFrame.Text = "0";
                _maxFrames.Content = string.Format("/ {0}", _maxCount - 1);

                if (_renderCheck.IsChecked == true)
                    Update3DView();
            }
        }

        private void _frameCount_TextInput(object sender, System.Windows.Input.TextCompositionEventArgs e)
        {
            int value = Convert.ToInt16(_frameCount.Content);
            if (value >= 0 && value + 1 < Math.Max(_depthCamFrameList.Count, _webCamFrameList.Count))
            {
                _currentFrame = value;
            }
        }

        private void _nextFrame_Click(object sender, RoutedEventArgs e)
        {
            if (_currentFrame + 1 < _maxCount)
                _currentFrame++;

            _frameCount.Content = _currentFrame.ToString();

            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);

            if (_renderCheck.IsChecked == true)
                Update3DView();
        }

        private void _prevFrame_Click(object sender, RoutedEventArgs e)
        {
            if (_currentFrame > 0)
                _currentFrame--;

            _frameCount.Content = _currentFrame.ToString();

            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);

            if (_renderCheck.IsChecked == true)
                Update3DView();
        }

        private void _firstFrame_Click(object sender, RoutedEventArgs e)
        {
            _currentFrame = 0;
            _frameCount.Content = "0";
            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);

            if (_renderCheck.IsChecked == true)
                Update3DView();
        }

        private void _lastFrame_Click(object sender, RoutedEventArgs e)
        {
            _currentFrame = _maxCount - 1;
            if (_currentFrame < 0) _currentFrame = 0;
            _frameCount.Content = _currentFrame.ToString();

            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);

            if (_renderCheck.IsChecked == true)
                Update3DView();
        }

        private void _selectFrame_SelectionChanged(object sender, System.Windows.Controls.SelectionChangedEventArgs e)
        {
            if (e.AddedItems != null && e.AddedItems.Count > 0)
            {
                _currentFrame = (int)e.AddedItems[0];
            }
            else
            {
                _currentFrame = 0;
            }

            _selectFrame.Text = _currentFrame.ToString();
            _frameCount.Content = _currentFrame.ToString();

            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);

            if (_renderCheck.IsChecked == true)
                Update3DView();
        }

        public void Play()
        {
            if (_playButton.IsChecked == true)
            {
                Play_Checked(true, null);
            }
            else
            {
                _selectFrame.IsEnabled = true;
                _playButton.IsChecked = false;
                _playButton.Content = "Play";
            }
        }

        private void Play_Checked(object sender, RoutedEventArgs e)
        {
            if (_currentFrame < _maxCount - 1) _currentFrame++;

            else
            {
                _playButton.IsChecked = false;
                _selectFrame.IsEnabled = true;
                TimingState endState = new TimingState();
                endState.IsActive = false;
                _playButton.Content = "Play";
                _mainPort.Post(new SetTiming(endState));

                return;
            }

            _playButton.Content = "Stop";

            _selectFrame.IsEnabled = false;

            ShowDepthCamFrame(_currentFrame);
            ShowWebCamFrame(_currentFrame);
            if (_renderCheck.IsChecked == true)
                Update3DView();

            _frameCount.Content = _currentFrame.ToString();

            TimingState state = new TimingState();
            state.IsActive = true;

            SetTiming setTimer = new SetTiming(state);
            _mainPort.Post(setTimer);
        }

        private void _playButton_Unchecked(object sender, RoutedEventArgs e)
        {
            _playButton.IsChecked = false;
            _selectFrame.IsEnabled = true;
            TimingState endState = new TimingState();
            endState.IsActive = false;
            _playButton.Content = "Play";
            _mainPort.Post(new SetTiming(endState));
        }

        private void _render3D_Click(object sender, RoutedEventArgs e)
        {
            Update3DView();
        }

        private void _3DViewGrid_MouseDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            if (e.LeftButton != MouseButtonState.Pressed) return;
            mDown = true;
            Point pos = Mouse.GetPosition(mainViewport);
            mLastPos = new Point(pos.X - mainViewport.ActualWidth / 2, mainViewport.ActualHeight / 2 - pos.Y);
        }

        private void _3DViewGrid_MouseMove(object sender, System.Windows.Input.MouseEventArgs e)
        {
            if (mDown)
            {
                Point pos = Mouse.GetPosition(mainViewport);
                Point actualPos = new Point(pos.X - mainViewport.ActualWidth / 2, mainViewport.ActualHeight / 2 - pos.Y);
                double dx = actualPos.X - mLastPos.X, dy = actualPos.Y - mLastPos.Y;
                

                double mouseAngle = 0;
                if (dx != 0 && dy != 0)
                {
                    mouseAngle = Math.Asin(Math.Abs(dy) / Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2)));
                    if (dx < 0 && dy > 0) mouseAngle += Math.PI / 2;
                    else if (dx < 0 && dy < 0) mouseAngle += Math.PI;
                    else if (dx > 0 && dy < 0) mouseAngle += Math.PI * 1.5;
                }
                else if (dx == 0 && dy != 0) mouseAngle = Math.Sign(dy) > 0 ? Math.PI / 2 : Math.PI * 1.5;
                else if (dx != 0 && dy == 0) mouseAngle = Math.Sign(dx) > 0 ? 0 : Math.PI;

                double axisAngle = mouseAngle + Math.PI / 2;

                Vector3D axis = new Vector3D(Math.Cos(axisAngle) * 4, Math.Sin(axisAngle) * 4, 0);

                double rotation = 0.001 * Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));

                Transform3DGroup group = mGeometry.Transform as Transform3DGroup;
                QuaternionRotation3D r = new QuaternionRotation3D(new System.Windows.Media.Media3D.Quaternion(axis, rotation * 180 / Math.PI));
                group.Children.Add(new RotateTransform3D(r));

                oldTransform = group;

                mLastPos = actualPos;
            }
        }

        private void _3DViewGrid_MouseUp(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            mDown = false;
        }

        private void _3DViewGrid_MouseWheel(object sender, System.Windows.Input.MouseWheelEventArgs e)
        {
            camera.Position = new Point3D(camera.Position.X, camera.Position.Y, camera.Position.Z + e.Delta / 250D);
        }

        #endregion

        private void Update3DView()
        {
            mesh = new MeshGeometry3D();
            
            var frame = _depthCamFrameList[_currentFrame];
            int pos = 0;
            for (int y = 0; y < frame.DepthImageSize.Height; y += 1)
            {
                for (int x = 0; x < frame.DepthImageSize.Width; x += 1)
                {
                    //get the world coordinates of the point relative to the kinect camera (0,0,0)
                    Vector3 point = MathUtilities.ConvertPixelSpaceToViewSpace(x, y, frame.DepthImage[pos++], frame.InverseProjectionMatrix, 320, 240, 4);

                    //discard unknown readings
                    if (point.X != 0 && point.Y != 0 && point.Z != 0)
                        AddTrihedron(mesh, new Point3D(-point.X, point.Y, point.Z), new Size3D(0.05, 0.05, 0.05), 0.1);
                }
            }

            mGeometry = new GeometryModel3D(mesh, frontMaterial);
            
            if (oldTransform == null)
            {
                oldTransform = new Transform3DGroup();
            }
            mGeometry.Transform = oldTransform;

            visualModel = new ModelVisual3D();
            visualModel.Content = mGeometry;

            if (mainViewport.Children == null)
            {
                this.mainViewport.Children.Add(visualModel);
            }
            else
            {
                //remove the old frame and load the new one
                mainViewport.Children.Add(visualModel);
                mainViewport.Children.Remove(_oldVisualModel);
            }

            _oldVisualModel = visualModel;
        }

        private void AddTrihedron(MeshGeometry3D mesh, Point3D p, Size3D size, double t)
        {
            Point3D top, bottom, a, b, c;

            top = new Point3D(p.X, p.Y, p.Z + (size.Z / 2.0));

            a = new Point3D(p.X, p.Y + (size.Y / 2.0), p.Z);
            b = new Point3D(p.X + 0.866 * (size.X / 2.0), p.Y + 0.5 * (size.Y / 2.0), p.Z);
            c = new Point3D(p.X - 0.866 * (size.X / 2.0), p.Y - 0.5 * (size.Y / 2.0), p.Z);

            bottom = new Point3D(p.X, p.Y, p.Z - (size.Z / 2.0));

            mesh.Positions.Add(top);
            mesh.TextureCoordinates.Add(new Point(t, 0));

            mesh.Positions.Add(a);
            mesh.TextureCoordinates.Add(new Point(t, 0));

            mesh.Positions.Add(b);
            mesh.TextureCoordinates.Add(new Point(t, 0));

            mesh.Positions.Add(c);
            mesh.TextureCoordinates.Add(new Point(t, 0));

            mesh.Positions.Add(bottom);
            mesh.TextureCoordinates.Add(new Point(t, 0));

            //AddPlane(mesh, b, top, a, bottom, t);
            mesh.TriangleIndices.Add(mesh.Positions.Count - 3); // a
            mesh.TriangleIndices.Add(mesh.Positions.Count - 5); // b
            mesh.TriangleIndices.Add(mesh.Positions.Count - 4); // c

            mesh.TriangleIndices.Add(mesh.Positions.Count - 4); // c
            mesh.TriangleIndices.Add(mesh.Positions.Count - 1); // d
            mesh.TriangleIndices.Add(mesh.Positions.Count - 3); // a

            //AddPlane(mesh, c, top, b, bottom, t);
            mesh.TriangleIndices.Add(mesh.Positions.Count - 2); // a
            mesh.TriangleIndices.Add(mesh.Positions.Count - 5); // b
            mesh.TriangleIndices.Add(mesh.Positions.Count - 3); // c

            mesh.TriangleIndices.Add(mesh.Positions.Count - 3); // c
            mesh.TriangleIndices.Add(mesh.Positions.Count - 1); // d
            mesh.TriangleIndices.Add(mesh.Positions.Count - 2); // a

            //AddPlane(mesh, a, top, c, bottom, t);
            mesh.TriangleIndices.Add(mesh.Positions.Count - 4); // a
            mesh.TriangleIndices.Add(mesh.Positions.Count - 5); // b
            mesh.TriangleIndices.Add(mesh.Positions.Count - 2); // c

            mesh.TriangleIndices.Add(mesh.Positions.Count - 2); // c
            mesh.TriangleIndices.Add(mesh.Positions.Count - 1); // d
            mesh.TriangleIndices.Add(mesh.Positions.Count - 4); // a
        }
    }
}